package robot.calicon;

import robot.calibeta.Motion;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;
import chair.Brain;
import chair.Organ;
import chair.core.Diary;

/**
 * Le robot explore son environnement et regarde s'il se trouve dans une cage
 * (aucun obstacle à plus de un mètre dans les quatres directions). Utilise deux
 * capteur : le bouton poussoir pour détecter si on l'a soulevé, et le capteur
 * ultrason pour détecter les obstacles.
 * 
 * Attention, comme on cherche à minimiser impact sur robots quand on le soulève
 * et qu'il n'y a pas de mécanismes prévus pour le moment (TODO) destiné à
 * rendre passif tous les autres organes, va monopoliser Stiratum tant que le
 * robot est en l'air (en arrêtant au passage les roues), pour ne rendre la main
 * qu'après être reposé et après avoir exploré.
 * 
 * NB: il faut lui ajouter la fonction gérant la motricité pour que puisse
 * tourner.
 */
public class OrganCage extends Organ {

	/** Soulevé ou non ? */
	private TouchSensor _somato;
	/** Permet de scruter ce qu'il y a autour */
	private UltrasonicSensor _visio;
	/**
	 * Le robot doit être enfermé dans un environnement dont les murs se
	 * trouvent à moins de 1 mètre pour qu'il considère cela comme une cage
	 */
	private final int CAGE_DISTANCE = 100;
	/**
	 * À quelle étape se situe le robot dans la démarche qui lui permet de
	 * regarder la cage ?
	 * 
	 * 1: a décollé ; 2: a demandé le verrou sur fonctions ; 3: verrou acquis,
	 * prêt à tourner ; 4: immobile et prêt à agir
	 * 
	 * 
	 */
	private int _defcon = 1;
	/** Le robot est-il revenu sur Terre ? */
	private boolean _hasLanded = true;

	/**
	 * La sensation actuelle de l'organe. Initialise à -1 car le robot ne peut
	 * pas déterminer dès le lancement du programme si se trouve dans une cage,
	 * doit d'abord posséder verrou avant de tourner.
	 */
	private byte _cage = -2;

	// FIXME: beaucoup trop de booléans pour éviter de monopoliser ressources

	public OrganCage(Brain brain) {
		super(brain, "OrganSomato");
		_somato = new TouchSensor(SensorPort.S4);
		_visio = new UltrasonicSensor(SensorPort.S2);
		// Une seule valeur possible : si détecte une cage
		byte[] values = { 1 };
		String[] names = { "DansUneCage" };
		registerValues(values, names);
	}

	/**
	 * S'occupe de tourner comme un derviche à la recherche d'une grille
	 * 
	 * @return true si considère que se trouve dans une cage, false sinon
	 */
	private boolean tasteCage() {
		// On présume qu'on a enfermé la créature
		boolean inACage = true;

		int obstacleDistance = _visio.getDistance();

		// Il faut que le robot regarde dans les quatre directions : besoin de tourner pour de vrai
		Motion.setRotateShortest(false);
		
		// Autant commencer par regarder ce qu'il y a devant
		Diary.logln("\nDistance obstacle devant : " + obstacleDistance);
		if (obstacleDistance > CAGE_DISTANCE)
			inACage = false;

		// Regarde ce qu'il y a à droite
		Motion.rotateFromCape(-90);
		obstacleDistance = _visio.getDistance();
		Diary.logln("\nDistance obstacle à droite : " + obstacleDistance);
		if (obstacleDistance > CAGE_DISTANCE)
			inACage = false;

		// Puis ce qu'il y a à derrière
		Motion.rotateFromCape(-180);
		obstacleDistance = _visio.getDistance();
		Diary.logln("\nDistance obstacle derrière : " + obstacleDistance);
		if (obstacleDistance > CAGE_DISTANCE)
			inACage = false;

		// Enfin : à gauche
		Motion.rotateFromCape(90);
		obstacleDistance = _visio.getDistance();
		Diary.logln("\nDistance obstacle à gauche : " + obstacleDistance);
		if (obstacleDistance > CAGE_DISTANCE)
			inACage = false;

		// Et remet le robot en place
		Motion.rotateFromCape(0);
		
		// À nouveau le robot reste droit comme un I
		Motion.setRotateShortest(true);

		Diary
				.logln("\nTerminé mon tour. Suis-je dans une cage ? ..."
						+ inACage);

		return inACage;
	}

	@Override
	protected byte sense() {
		// Le robot a décollé et n'a pas encore cherché à verrouiller fonctions
		if (_defcon == 1) {
			Diary.logln(this + "on a décollé");
			// On cherche à obtenir contrôle sur les roues
			callLock();
			// Le verrou va être appelé, on ne veut surtout pas rappeler avant
			// nos affaires
			_defcon++;
		}
		// Si pas encore le verrou, on teste
		if (_defcon == 2) {
			if (hasLock()) {
				Diary.logln(this + "on a le verrou");
				_defcon++;
			}
		}
		// Le robot est à nous : stoppe mouvements
		if (_defcon == 3) {
			Diary.logln(this + "stoppe robot");
			// Arrête le robot
			Motion.stop();
			_defcon++;
		}
		// Prêt et a atterri : on est parti
		if (_defcon == 4 && _hasLanded) {
			Diary.logln("prêt à tourner");
			_cage = tasteCage() ? (byte) 1 : 0;

			// Terminé: drapeau blanc et éteint alerte.
			_hasLanded = false;
			_defcon = 0;

			// Et on oublie pas de rendre le verrou
			releaseLock();
		}

		// Il faut surveiller capteur tactile, pour ne pas encombrer canal on ne
		// mesure rien quand robot tourne
		if (!Motion.isWorking()) {
			// Là, tout de suite, où est le robot ?
			boolean isInTheAir = !_somato.isPressed();
			// Robot vient d'attérir
			if (_defcon > 0 && !isInTheAir)
				_hasLanded = true;
			// Vient de décoller
			if (_defcon == 0 && isInTheAir)
				_defcon++;
		}

		return _cage;
	}
}
